/*
 * @Author: LVGRAPE
 * @LastEditors: LVGRAPE
 */
/*
 * @Author: LVGRAPE
 * @LastEditors: LVGRAPE
 */
#include <rtthread.h>
#include "drv_led.h"
#include "drv_serial.h"
#include "drv_pwm.h"
#include "drv_soft_i2c.h"
#include "drv_spi.h"
#include "icm42688p_read_polling.h"
#include "nds03_api.h"
#include "pwm3901_polling.h"
#include "spl06_api.h"
#include "rc.h"
#include "zino.h"
#include "fc_state.h"
#include "anoPTv8_report.h"
#include "imu.h"
#include "fc_sensors.h"
#include "ano_cmd.h"
#include "rc_controls.h"
#include "motor_mix.h"
#include "drv_adc.h"
#include "attitude.h"
#include "estimator.h"
#include "HWInterface.h"
#include "position.h"
#include "fc.h"
#define DBG_TAG "FC"
#define DBG_LVL DBG_LOG
#include <rtdbg.h>

extern int adc_init(void);
extern void adc_show_value(void);
static uint16_t _1msCnt = 0;
static uint8_t c;
static uint32_t curent_ms;

m_imu_t fc_ctrl_imu = {
    .kp = 0.25f, // 0.25
    .ki = 0.01f,
    .q0 = 1.0f,
    .q1 = 0.f,
    .q2 = 0.f,
    .q3 = 0.f,
};

int fc_task_init(void)
{
    int ret = 0;
    pwm_init(); // motors, leds
    LOG_I("motors, leds pwm init");
    at32_tmr_pwm_dma_init(); // rgbs
    LOG_I("rgbs pwm init");
    adc_init(); // current, voltage
    LOG_I("adc init");
    soft_i2c1_init(); // i2c
    LOG_I("i2c init");
    spi1_gpio_config(); // spi
    LOG_I("spi init");

    rt_thread_mdelay(1);
    // soft_i2c1_scan();

#if USED_TOF
    ret = nds03_init();
    if (ret != RT_EOK)
    {
        FC_STATE_SET(FC_LASER_HARDFAULT);
        LOG_E("nds03 init failed: %d", ret);
    }
    else
    {
        LOG_I("nds03 init success");
    }
    rt_thread_mdelay(1);
#endif

    ret |= pwm3901_dev_init();
    if (ret != RT_EOK)
    {
        FC_STATE_SET(FC_OPTICALFLOW_HARDFAULT);
        LOG_E("pwm3901 init failed: %d", ret);
    }
    else
    {
        LOG_I("pwm3901 init success");
    }

    ret = SPL06_dev_Init();
    if (ret != RT_EOK)
    {
        FC_STATE_SET(FC_BARO_HARDFAULT);
        LOG_E("SPL06 init failed: %d", ret);
    }
    else
    {
        LOG_I("SPL06 init success");
    }
    rt_thread_mdelay(1);

    ret = icm42688p_read_polling_init();
    if (ret != RT_EOK)
    {
        FC_STATE_SET(FC_ACC_GYRO_HARDFAULT);
        LOG_E("icm42688p init failed: %d", ret);
    }
    else
    {
        LOG_I("icm42688p init success");
        sensor_acc_init(0);
        sensor_gyro_init(0);
    }
    rt_thread_mdelay(1);

    ret = rc_task_init();
    if (ret != RT_EOK)
    {
        LOG_E("rc_task init failed: %d", ret);
    }
    else
    {
        LOG_I("rc_task init success");
    }

    return ret;
}
void fc_task_routine(void *parameter)
{

    // static uint32_t _1msT = 0;

    rt_hw_uart4_init();

    fc_task_init();

    rc_control_max_init();
    attiude_controler_init();
    estimator_init();
    opticalflow_sensor_handle_init();
    position_z_pid_init();
    position_xy_pid_init();

    motor_led_state_set(0, RGB_STATE_MODE_BREATH);
    motor_led_state_set(1, RGB_STATE_MODE_BREATH);
    motor_led_state_set(2, RGB_STATE_MODE_BREATH);
    motor_led_state_set(3, RGB_STATE_MODE_BREATH);

    anoPTv8_init();
    ano_cmd_init();
    while (1)
    {
        curent_ms = millis();
        _1msCnt++;

        anoPTv8_task_1ms();
        acc_update();
        gyro_update();
        rc_task_1ms();
        attitude_controler();
        motor_mix();
        pwm3901_update_1ms();

        if (uart4_read(&c, 1) == 1)
        {
            AnoPTv8HwRecvByte(c);
        }

        if (_1msCnt % 6 == 0)
        {
            rcControlsUpate();
        }

        if (_1msCnt % 10 == 0)
        {
            static uint32_t dt_10ms_Pre = 0;
            float dt_10ms = (float)(curent_ms - dt_10ms_Pre) * 0.001f;
            dt_10ms_Pre = curent_ms;
            MahonyAHRS(&fc_ctrl_imu, &fc_sensor_data, dt_10ms);
            sensor_baro_updata();
            opticalflow_sensor_handle();
            estimator_update(dt_10ms);
            keyScanPro_10ms();
            adc_calculate();
            position_z_control(dt_10ms);
            position_xy_control(dt_10ms);
        }
        if (_1msCnt % 50 == 0)
        {
            generate_led_blink_value();
            fc_state_show();
            // rt_kprintf("Rc: %02X, %02X\n",rchandle.rx_data.v.channel[5], rchandle.rx_data.v.channel[6]);
        }
        if (_1msCnt % 33 == 0)
        {
            tof_update();
        }

        if (_1msCnt % 1000 == 0)
        {
            rc_task_1s();
            // fc_state_check();
        }

        if (_1msCnt >= 1000)
        {
            _1msCnt = 0;
            // uart4_write("hello world\r\n", 14);
        }

        // rt_schedule(); //让出cpu调用更高优先级的线程，无法让出
        rt_thread_mdelay(1); // 只能用延时让出CPU，不能yield，否则会导致系统卡死
        // rt_thread_yield();//让出cpu调用更高优先级的线程, 此线程为最高优先级，无法让出
    }
}
static uint8_t fc_thread_stack[1024 * 4];
struct rt_thread fc_thread;
int fc_task_thread_init(void)
{
    rt_thread_t tid = &fc_thread;
    rt_err_t ret = rt_thread_init(
        tid,                     // 线程控制块指针
        "fc",                    // 线程名称
        fc_task_routine,         // 线程入口函数
        RT_NULL,                 // 线程入口函数参数
        fc_thread_stack,         // 线程栈起始地址
        sizeof(fc_thread_stack), // 线程栈大小
        0,                       // 线程的优先级
        1                        // 线程调度策略
    );
    if (ret != RT_EOK)
    {
        LOG_E("fc thread init failed: %d", ret);
        return ret;
    }
    ret = rt_thread_startup(tid);
    if (ret != RT_EOK)
    {
        LOG_E("fc thread startup failed: %d", ret);
    }
    return ret;
}
